
#include "base/utils/utils.h"
#include "base/types/SO3.h"
#include "network/Socket++.h"
#include "hardware/Gps/utils_GPS.h"
#include "base/utils/utils_math.h"

#include "utils_mavlink.h"
#include "VirtualUAV_Fixedwing.h"


using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

#define PI          (3.1415926)
#define D2R         (3.1415926 / 180.0)
#define R2D         (180.0 / 3.1415926)
#define M2FT        (0.3048)                //transfer (m/sec^2) to (ft/sec^2)
#define R_EARTH     (6378100.0)

#define METER2FEET  3.2808                  // meter to feet

////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

VirtualUAV_Fixedwing::VirtualUAV_Fixedwing()
{
    init();
}

VirtualUAV_Fixedwing::~VirtualUAV_Fixedwing()
{
    release();
}

int VirtualUAV_Fixedwing::init()
{
    string uavName = fmt::sprintf("vuav_%d", ID);

    uavType         = MAV_TYPE_FIXED_WING;

    sim_m           = svar.GetDouble(uavName + ".m", 10.0);

    sim_Ax          = 0;
    sim_Ay          = 0;
    sim_Az          = 0;
    sim_Vx          = 0;
    sim_Vy          = 0;
    sim_Vz          = 0;

    sim_tLast       = 0;
    sim_tNow        = 0;
    sim_totalThrust = 0;

    m_simStep       = 0;

    svar.GetInt("UAS.UAV.uavStatus", 0) = 0;

    m_numMotor      = 1;
    for(int i=0; i<m_numMotor; i++) {
        svar.GetInt(fmt::sprintf("UAS.UAV.eng_state_%d", i), 2) = 0;
        svar.GetDouble(fmt::sprintf("UAS.UAV.rpm_%d", i), 2000) = 0;
    }

    // set start position settings
    m_startPOS_set = 0;
    m_startPOS_lat = 0;
    m_startPOS_lng = 0;
    m_startPOS_alt = 0;

    m_engineStart = 0;
    m_engineStartTM = 0;

    m_tmLastAltSet = 0;

    // start JSBSim
    m_jsbSim.m_jsbScript = svar.GetString("VirtualUAV.fixedWing.modelName", "c1723_manual.xml");
    //m_jsbSim.m_jsbScript = "c1723.xml";
    m_jsbSim.run();

    return 0;
}

int VirtualUAV_Fixedwing::release()
{
    // stop JSBSim
    m_jsbSim.stop();

    return 0;
}

int VirtualUAV_Fixedwing::timerFunction(void *arg)
{
    return VirtualUAV::timerFunction(arg);
}

//
// joystick defines
//
//  jsv->AXIS[0];         // roll
//  jsv->AXIS[1];         // pitch
//  jsv->AXIS[2];         // thrust
//  jsv->AXIS[3];         // yaw
//
int VirtualUAV_Fixedwing::simulation(pi::JS_Val *jsv)
{
    const int   jsN = JS_AXIS_MAX_NUM;
    float       js[jsN];

    double      dt;
    int         i;


    // time
    sim_tNow = tm_getTimeStamp();
    dt = sim_tNow - sim_tLast;
    if( dt > 100 ) {
        sim_tLast = sim_tNow;
        return -1;
    }
    sim_tLast = sim_tNow;

    updateTime(sim_tNow);

    // process input axis values
    svar.GetDouble("UAS.UAV.num_controlInput", jsN) = jsN;

    for(i=0; i<jsN; i++) {
        js[i] = jsv->AXIS[i];

        svar.GetDouble(fmt::sprintf("UAS.UAV.controlInput_%d", i), 0) = js[i];

        //if( fabs(js[i]) < js_deadzone ) js[i] = 0.0;
    }

    // send command to JSBSim - set control surfaces
    m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "fcs/aileron-cmd-norm",  jsv->AXIS[0]));
    m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "fcs/elevator-cmd-norm", -jsv->AXIS[1]));
    m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "fcs/rudder-cmd-norm",   jsv->AXIS[3]));
    m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "fcs/throttle-cmd-norm", (jsv->AXIS[2]+1)/2.0));

    /*
    printf("jsv: ");
    for(i=0; i<4; i++) printf("[%2d]: %6.2f ", i, jsv->AXIS[0]);
    printf("\n");
    */

    // read simulation results
    // receive JSBSim results
    std::vector<double> dat;
    if( 0 != m_jsbSim.recvData(dat) ) return -1;

    double simTime;
    double simLat, simLng, simAlt;
    double simRoll, simPitch, simYaw;

    simTime     = dat[JSBSIM_DAT_TIME];
    simLat      = dat[JSBSIM_DAT_LATITUDE];
    simLng      = dat[JSBSIM_DAT_LONGITUDE];
    simAlt      = dat[JSBSIM_DAT_ALTITUDE];

    simRoll     = dat[JSBSIM_DAT_PHI];
    simPitch    = dat[JSBSIM_DAT_THT];
    simYaw      = dat[JSBSIM_DAT_PSI];

    // first message?
    if( m_startPOS_set == 0 ) {
        if( fabs(simLat-svar.GetDouble("VirtualUAV.JSBSim.beginLat", 34.0)) > 1e-3 ||
            fabs(simLng-svar.GetDouble("VirtualUAV.JSBSim.beginLng", 112.0)) > 1e-3 )
            return -2;

        m_startPOS_set = 1;
        m_startPOS_lat = simLat;
        m_startPOS_lng = simLng;
        m_startPOS_alt = simAlt/METER2FEET;

        printf("start pos [%9.2f]: %12.6f %12.6f : %9.2f\n",
               simTime,
               m_startPOS_lat, m_startPOS_lng, m_startPOS_alt);
    } else {
        /*
        printf("new pos [%9.2f]  : %12.6f %12.6f : %9.2f\n",
               simTime,
               simLat, simLng, simAlt);
        */

        simLat = simLat - m_startPOS_lat;
        simLng = simLng - m_startPOS_lng;
        simAlt = simAlt/METER2FEET - m_startPOS_alt;
    }


    // detect landgear button
    if( fabs(jsv->BUTTON[svar.GetInt("VirtualUAV.landgearBtn", 2)]) > 0.001 ) {
        if( sim_tNow - m_landgearTM > svar.GetDouble("VirtualUAV.landgearTimeInterval", 1) ) {
            if( m_landgearState ) m_landgearState = 0;
            else                  m_landgearState = 1;

            m_landgearTM = sim_tNow;

            svar.GetInt("UAS.UAV.landgearState", 0) = m_landgearState;
        }
    }

    // start engine (arm)
    if( m_engineStart ) {
        if( sim_tNow - m_engineStartTM > 6 && m_engineStartTM != -1 ) {
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "fcs/left-brake-cmd-norm", 0));
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "fcs/right-brake-cmd-norm", 0));
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "fcs/center-brake-cmd-norm", 0));

            //m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "fcs/flap-cmd-norm", 0.33));

            m_engineStartTM = -1;
        }
    } else {
        if( fabs(jsv->BUTTON[svar.GetInt("VirtualUAV.armBtn", 10)]) > 0.001 ) {
            m_engineStartTM = sim_tNow;
            m_engineStart = 1;

            setArmed(1);
            setFlightMode(MFM_STABILIZE);

            m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "fcs/mixture-cmd-norm", 1.0));

            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "propulsion/magneto_cmd", 3));
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "propulsion/starter_cmd", 1));

            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "ap/roll-attitude-mode", 1));
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "ap/autopilot-roll-on", 1));
        }
    }


    // update VirtualUAV information
    roll            = simRoll;
    pitch           = simPitch;
    yaw             = simYaw;

    gpLat           = homeLat + simLat;
    gpLon           = homeLng + simLng;
    gpH             = simAlt;
    gpAlt           = homeAlt + gpH;
    gpHeading       = simYaw;

    lat             = gpLat;
    lon             = gpLon;
    alt             = gpAlt;


    gpsGroundSpeed  = sqrt(sim_Vx*sim_Vx + sim_Vy*sim_Vy);
    gpsFixType      = 3;

    if( m_simStep % 100 == 0 ) {
        cpuLoad         = 50.0 + (rand() % 20 - 10);

        nSat            = 20;
        HDOP_h          = 1 + 0.004*(rand()%100 - 50);
        HDOP_v          = 1.5 + 0.01*(rand()%100 - 50);

        /*
        double rssi = 100 + (1.0-1.0/(1+exp(-P_earth.norm()/1000))) * 200 + 0.1*(rand()%100-50);
        svar.GetDouble("UAS.Telem.RSSI", 100) = rssi;
        rssi = 100 + (1.0-1.0/(1+exp(-P_earth.norm()/1000))) * 200 + 0.1*(rand()%100-50);
        svar.GetDouble("UAS.Video.RSSI", 100) = rssi;
        */

        battVolt        = svar.GetDouble("UAS.UAV.battVolt", 24.5) + 0.01*(rand() % 100 - 50);
        //battCurrent     = V_earth.norm() + 0.01*(rand() % 100 - 50);
        if( battCurrent < 0 ) battCurrent = 0;
        battRemaining   = 80.0;
        commDropRate    = 0.0;
    }

    if( m_engineStart && customMode == MFM_AUTO ) {
        autoNavigate(js);
    } else {

    }


    return 0;
}

int VirtualUAV_Fixedwing::autoNavigate(float *js)
{
    static double tsCmdLast = 0;
    double tmNow = tm_getTimeStamp();

    // check mission array
    if( m_missionArray.size() <= 0 ) {
        setFlightMode(MFM_STABILIZE);            // change flight mode to 'stablize'
        return 0;
    }

    // calculate distance to target
    double              dx, dy, gd, dz;
    MissionData_Simp    &md = m_missionArray[m_missionIndex];

    calcLngLatDistance(gpLon, gpLat, md.lng, md.lat,
                       dx, dy);
    dz = md.alt - gpH;                          // height difference
    gd = sqrt(dx*dx + dy*dy);                   // ground distance

    // goto next waypoint
    //if( sqrt(dx*dx+dy*dy+dz*dz) < svar.GetDouble("VirtualUAV.autoNavigate.distThreshold", 5.0) ) {
    if( sqrt(dx*dx+dy*dy) < svar.GetDouble("VirtualUAV.autoNavigate.distThreshold", 15.0) ) {
        m_missionIndex ++;
        if( m_missionIndex >= m_missionNum ) {
            m_missionIndex = 0;
            setFlightMode(MFM_STABILIZE);       // change flight mode to 'stablize'
        }

        return 0;
    }

    if( tmNow - tsCmdLast > 0.5 ) {
        double hd = -atan2(dy, dx) + M_PI/2.0;
        m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "guidance/specified-heading-rad", hd));
        m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "ap/roll-attitude-mode", 1));

        double hm = (m_startPOS_alt+md.alt)*METER2FEET;
        if( tmNow - m_tmLastAltSet > 10.0 ) {
            m_jsbSim.sendMsg(fmt::sprintf("set %s %f\r\n", "ap/altitude_setpoint", hm));
            m_jsbSim.sendMsg(fmt::sprintf("set %s %d\r\n", "ap/altitude_hold", 1));
            m_tmLastAltSet = tmNow;
        }

        printf("target: %9.2f, %9.3f\n", hm, hd);

        tsCmdLast = tmNow;
    }

    return 0;
}


int VirtualUAV_Fixedwing::toFlightGear(FGNetFDM *fgData)
{
    FGNetFDM &fdm = *fgData;

    fdm.phi                 = convByteOrder_h2n<float>(roll  * D2R );
    fdm.theta               = convByteOrder_h2n<float>(pitch * D2R );
    fdm.psi                 = convByteOrder_h2n<float>(yaw   * D2R );

    // X, Y, Z accel in body frame ft/sec^2
    fdm.A_X_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_X_pilot * M2FT);
    fdm.A_Y_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_Y_pilot * M2FT);
    fdm.A_Z_pilot           = convByteOrder_h2n<float>(m_flightGearData.A_Z_pilot * M2FT);

    fdm.v_body_u            = convByteOrder_h2n<float>(m_flightGearData.v_body_u);
    fdm.v_body_v            = convByteOrder_h2n<float>(m_flightGearData.v_body_v);
    fdm.v_body_w            = convByteOrder_h2n<float>(m_flightGearData.v_body_w);

    fdm.longitude           = convByteOrder_h2n<double>(m_flightGearData.longitude * D2R);
    fdm.latitude            = convByteOrder_h2n<double>(m_flightGearData.latitude  * D2R);
    fdm.altitude            = convByteOrder_h2n<double>(m_flightGearData.altitude);

    float rpm               = (sim_totalThrust/sim_m)*1000;
    fdm.num_engines         = convByteOrder_h2n<uint32_t>(4);
    fdm.rpm[0]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[1]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[2]              = convByteOrder_h2n<float>(rpm);
    fdm.rpm[3]              = convByteOrder_h2n<float>(rpm);

    fdm.num_tanks           = convByteOrder_h2n<uint32_t>(1);
    fdm.fuel_quantity[0]    = convByteOrder_h2n<float>(100.0);

    fdm.num_wheels          = convByteOrder_h2n<uint32_t>(3);

    fdm.cur_time            = convByteOrder_h2n<uint32_t>(tm_getTimeStampUnix());
    fdm.warp                = convByteOrder_h2n<uint32_t>(1);

    fdm.visibility          = convByteOrder_h2n<float>(m_flightGearData.visibility);

    fdm.version             = convByteOrder_h2n<uint32_t>(FG_NET_FDM_VERSION);

    return 0;
}
